11. Look Away
Look Away
To see a Subscriber in action, you'll write a node called
look_away
. The
look_away
node will subscribe to the
/rgb_camera/image_raw
topic, which has image data from the camera mounted on the end of the robotic arm. Whenever the camera is pointed towards an uninteresting image - in this case, an image with uniform color - the callback function will move the arm to something more interesting. There are a few extra pieces in the code to ensure that this procedure is executed smoothly, but you will learn more about them later.
Creating the empty look_away node script
Just as before when you created the
arm_mover
and
simple_mover
nodes, you can create the
look_away node
as follows:
$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ touch look_away
$ chmod u+x look_away
Let's have a look at the code for
look_away
.
Troubleshooting look_away
In some cases
look_away
is executing when running it manually but is not executing automatically with
roslaunch
. This is typically a timing issue. If
look_away
starts before the system has fully initialized, then
look_away
hangs in the call to
safe_move
. Student
jsteinbae
offered a great solution to this issue:
My workaround was to add
wait_for_message
to the look_away node before subscribing to the topics.
This ensures that the callbacks are not called before the gazebo simulation (publishing these topics) is fully initialized.
def __init__(self):
rospy.init_node('look_away')
self.last_position = None
self.arm_moving = False
rospy.wait_for_message('/simple_arm/joint_states', JointState)
rospy.wait_for_message('/rgb_camera/image_raw', Image)
self.sub1 = rospy.Subscriber('/simple_arm/joint_states',
JointState, self.joint_states_callback)
self.sub2 = rospy.Subscriber('/rgb_camera/image_raw',
Image, self.look_away_callback)
self.safe_move = rospy.ServiceProxy('/arm_mover/safe_move',
GoToPosition)
rospy.spin()
Updating the launch file
Just as you did with the
arm_mover
node, to get
look_away
to launch with the rest of the nodes, you will need to modify
robot_spawn.launch
, which can be found in
~/catkin_ws/src/simple_arm/launch
. You can add the following code there:
<!-- The look away node -->
<node name="look_away" type="look_away" pkg="simple_arm"/>
While editing this file, it will be helpful to set
max_joint_2_angle: 1.57
in
arm_mover
so that it isn't necessary to set it again from the command line:
<!-- The arm mover node -->
<node name="arm_mover" type="arm_mover" pkg="simple_arm">
<rosparam>
min_joint_1_angle: 0
max_joint_1_angle: 1.57
min_joint_2_angle: 0
max_joint_2_angle: 1.57
</rosparam>
</node>